//
// Created by luzhan on 19-10-4.
//

#include "targetprediction.hpp"
#include "timer.hpp"
const double PI = 3.14159;
using namespace cv;
using std::cout;

TargetPrediction::TargetPrediction() {
    direction = UNKNOWN;
    angle_offset = 10.0;
}

TargetPrediction::~TargetPrediction() {
}

void TargetPrediction::toPolarCoordinates(cv::Point current_point, cv::Point origin_point, double R) {
    Timer clock;
    //x轴从左向右，y轴从下到上
    Point new_point = Point(current_point.x - origin_point.x, origin_point.y - current_point.y);
    if (new_point.y >= 0)
        angle = acos(new_point.x / R) * 180 / PI;
    else
        angle = 360 - acos(new_point.x / R) * 180 / PI ;
    if (angle_array.size() < 20){
        angle_array.emplace_back(angle);
    }else{
        direction = judgeDirection();
        angle_array.erase(angle_array.begin());
        angle_array.emplace_back(angle);
    }
    cout<<"极坐标换算    ";
    clock.stop();
}

//判断风车转向
//风车转速 10rad/min = 60°/s     视频素材FPS 25帧/s
// 60 / 25 = 2.4°/帧
int TargetPrediction::judgeDirection() {
    Timer clock;
    size_t i;
    int CW_cnt = 0;
    int CCW_cnt = 0;
    int STATIC_cnt = 0;
    double delta;
    for (i = 1; i < angle_array.size(); ++i){
        delta = angle_array[i] - angle_array[i-1];
        //对于角度在x轴正半轴附近的跳变进行补偿
        if (delta < -180) delta += 360;
        if (delta > 180) delta -= 360;
        if (abs(delta) < 1)
            STATIC_cnt++;
        else if (delta > 0)
            CCW_cnt++;
        else
            CW_cnt++;
    }
    cout<<"判断方向    ";
    clock.stop();
    if (CW_cnt > 12)
        return CW;
    else if (CCW_cnt > 12)
        return CCW;
    else if (STATIC_cnt > 12)
        return STATIC;
    else
        return UNKNOWN;
}

void TargetPrediction::predicting(Energy &energy) {
    Timer clock;
    double next_angle;
    double dx, dy;
    switch (direction){
        case UNKNOWN:
            return;
            break;
        case CW:
            next_angle = angle - angle_offset;
            break;
        case CCW:
            next_angle = angle + angle_offset;
            break;
        case STATIC:
            next_angle = angle;
    }
    if (next_angle > 360) next_angle -= 360;
    if (next_angle < 0) next_angle += 360;
    dx = cos(next_angle * PI / 180) * energy.circle_radius;
    dy = sin(next_angle * PI / 180) * energy.circle_radius;
    //从极坐标系转换到图像坐标系
    energy.predicted_point.x = energy.circle_center.x + dx;
    energy.predicted_point.y = energy.circle_center.y - dy;
    cout<<"位置预测    ";
    clock.stop();
}